/*******************************************************************************************
// routines only to the RM autopilot
//  Current Version is R3.01
//  Most are still in the R2 spec as well
//*****************************************************************************************/
void process_pilot_message(void)
{	int buffer_size;
	unsigned int bytes_received;

// NOTE: the commented structs are NOT needed here, but if we ever start usting them, then
	union								
  { char raw[MAX_MESSAGE_SIZE];
	struct msg_state_t state_field;
	struct msg_ppm_t ppm_field;
	struct msg_gps_t gps_field;
	struct msg_autopilot_t autopilot_field;
	struct msg_afcs_t afcs_field;
	//struct msg_tangent_plane_t tangent_plane_field;
	//struct msg_desired_pos_t desired_pos_field;
	//struct msg_flyto_t flyto_field;
	struct x_gain_msg_t x_gain_field;
	struct y_gain_msg_t y_gain_field;
	struct z_gain_msg_t z_gain_field;
	} network_buffer;
	
  struct sockaddr_in Source;			// allocate a structure to hold the source info
  memset(&network_buffer, 0, sizeof(network_buffer));
  memset(&Source, 0, sizeof(Source));
  buffer_size = sizeof( Source );		// get the size of the packet source buffer
#ifdef WINDOWS
  bytes_received = recvfrom( pilot_socket_fd, (char*)&network_buffer, sizeof( network_buffer ), 0,(struct sockaddr*)&Source, &buffer_size );
#endif
#ifdef LINUX
  bytes_received = recvfrom( pilot_socket_fd, (void*)&network_buffer, sizeof( network_buffer ), 0,(struct sockaddr*)&Source, (socklen_t*)&buffer_size );
#endif
  if( bytes_received == INVALID )
  {   bytes_received = 0;
	  if( errno != 11 )					// trap EAGAIN / EWOULODBLOCK / Resource temporarily unavailable
	  { printf(" %d -> ", errno );
		perror( "process_pilot_message:recvfrom -> " );	  }
  }
  if( bytes_received > 0 )
  { 
// stuffing the memory structures is here, but ONLY the ones USED are left to run.
// the system has no trouble doing them all, but its best to keep them short
      switch( network_buffer.raw[11] )
      { 
      case STATE_MSG:							// process msg_state_t packet
		uavi_telemetry.phi = 57.2*ntohf( network_buffer.state_field.phi );
        uavi_telemetry.theta = 57.2*ntohf( network_buffer.state_field.theta );
        uavi_telemetry.psi = 57.2*ntohf( network_buffer.state_field.psi );
	    //uavi_telemetry.p = ntohf( network_buffer.state_field.p );
        //uavi_telemetry.q = ntohf( network_buffer.state_field.q );
        //uavi_telemetry.r = ntohf( network_buffer.state_field.r );
        uavi_telemetry.x = ntohf( network_buffer.state_field.x );
        uavi_telemetry.y = ntohf( network_buffer.state_field.y );
        uavi_telemetry.z = ntohf( network_buffer.state_field.z );
        uavi_telemetry.vx = ntohf( network_buffer.state_field.vx );
        uavi_telemetry.vy = ntohf( network_buffer.state_field.vy );
        uavi_telemetry.vz = ntohf( network_buffer.state_field.vz );
        //uavi_telemetry.mx = ntohf( network_buffer.state_field.mx );
        //uavi_telemetry.my = ntohf( network_buffer.state_field.my );
        //uavi_telemetry.mz = ntohf( network_buffer.state_field.mz );
	    //uavi_telemetry.ax = ntohf( network_buffer.state_field.ax );
        //uavi_telemetry.ay = ntohf( network_buffer.state_field.ay );
        //uavi_telemetry.az = ntohf( network_buffer.state_field.az );
        //uavi_telemetry.raw_p = ntohf( network_buffer.state_field.raw_p );
        //uavi_telemetry.raw_q = ntohf( network_buffer.state_field.raw_q );
        //uavi_telemetry.raw_r = ntohf( network_buffer.state_field.raw_r );
        //uavi_telemetry.ahrs_trace = ntohf( network_buffer.state_field.ahrs_trace );
	    uavi_telemetry.imu_voltage = ntohf( network_buffer.state_field.voltage);
		uavi_telemetry.resets = ntohs( network_buffer.state_field.resets );
//	    uavi_telemetry.rpm0 = ntohs( network_buffer.state_field.rpm0);
//		uavi_telemetry.rpm1 = ntohs( network_buffer.state_field.rpm1);
//		uavi_telemetry.rpm2 = ntohs( network_buffer.state_field.rpm2);
		break;
	case PPM_MSG:	// process ppm in command packet
//		uavi_telemetry.valid = ntohs(network_buffer.ppm_field.valid);
//		uavi_telemetry.roll_ppm = ntohs(network_buffer.ppm_field.roll_ppm);
//		uavi_telemetry.pitch_ppm = ntohs(network_buffer.ppm_field.pitch_ppm);
//		uavi_telemetry.throttle_ppm = ntohs(network_buffer.ppm_field.throttle_ppm);
//		uavi_telemetry.yaw_ppm = ntohs(network_buffer.ppm_field.yaw_ppm);
//		uavi_telemetry.yaw_gain_ppm = ntohs(network_buffer.ppm_field.yaw_gain_ppm);
//		uavi_telemetry.coll_ppm = ntohs(network_buffer.ppm_field.coll_ppm);
		uavi_telemetry.ppm_mode = ntohs(network_buffer.ppm_field.ppm_mode);
//		uavi_telemetry.extra1 = ntohs(network_buffer.ppm_field.extra1);
//		uavi_telemetry.extra2 = ntohs(network_buffer.ppm_field.extra2);
		uavi_telemetry.auto_manual = ntohs(network_buffer.ppm_field.auto_manual);
//		uavi_telemetry.idle = ntohs(network_buffer.ppm_field.idle);
//		uavi_telemetry.gyro_gain = ntohs(network_buffer.ppm_field.gyro_gain);
		uavi_telemetry.crc_err = ntohs(network_buffer.ppm_field.crc_err);
		uavi_telemetry.glitches = ntohs(network_buffer.ppm_field.glitches);
//		uavi_telemetry.mask = ntohs(network_buffer.ppm_field.mask);
		break;
	case GPS_MSG:	//gps status 
		uavi_telemetry.numsv = ntohs(network_buffer.gps_field.numsv);
		uavi_telemetry.sacc = ntohs(network_buffer.gps_field.sacc);
		uavi_telemetry.pacc = ntohs(network_buffer.gps_field.pacc);
//		uavi_telemetry.fixtype = ntohs(network_buffer.gps_field.fixtype);
//		uavi_telemetry.vel_n_gps = ntohf( network_buffer.gps_field.vel_n_gps);
//		uavi_telemetry.vel_e_gps = ntohf( network_buffer.gps_field.vel_e_gps);
//		uavi_telemetry.vel_d_gps = ntohf( network_buffer.gps_field.vel_d_gps);
//		uavi_telemetry.pos_n_gps = ntohf( network_buffer.gps_field.pos_n_gps);
//		uavi_telemetry.pos_e_gps = ntohf( network_buffer.gps_field.pos_e_gps);
//		uavi_telemetry.pos_d_gps = ntohf( network_buffer.gps_field.pos_d_gps);
//		uavi_telemetry.raw_pos_n = ntohf( network_buffer.gps_field.raw_pos_n);
//		uavi_telemetry.raw_pos_e = ntohf( network_buffer.gps_field.raw_pos_e);
//		uavi_telemetry.raw_pos_d = ntohf( network_buffer.gps_field.raw_pos_d);
		uavi_telemetry.gps_trace = ntohf( network_buffer.gps_field.gps_trace);	
		break;
	case  AUTOPILOT_MSG:   //autopilot values from pilot
		uavi_telemetry.manual_ap = ntohs( network_buffer.autopilot_field.manual_ap);
		uavi_telemetry.mode_ap = ntohs( network_buffer.autopilot_field.mode_ap);
//		uavi_telemetry.roll_ap = ntohs( network_buffer.autopilot_field.roll_ap);
//		uavi_telemetry.pitch_ap = ntohs( network_buffer.autopilot_field.pitch_ap);
//		uavi_telemetry.yaw_ap = ntohs(network_buffer.autopilot_field.yaw_ap);
//		uavi_telemetry.throttle_ap = ntohs(network_buffer.autopilot_field.throttle_ap);
//		uavi_telemetry.coll_ap = ntohs(network_buffer.autopilot_field.coll_ap);
//		uavi_telemetry.padding_ap = ntohs(network_buffer.autopilot_field.padding_ap);
		uavi_telemetry.n_d_ap = ntohf(network_buffer.autopilot_field.n_d_ap);
		uavi_telemetry.e_d_ap= ntohf(network_buffer.autopilot_field.e_d_ap);
		uavi_telemetry.d_d_ap = ntohf(network_buffer.autopilot_field.d_d_ap);
//		uavi_telemetry.vx_d_ap = ntohf( network_buffer.autopilot_field.vx_d_ap );
//        uavi_telemetry.vy_d_ap = ntohf( network_buffer.autopilot_field.vy_d_ap );
//        uavi_telemetry.vz_d_ap = ntohf( network_buffer.autopilot_field.vz_d_ap );
       	//uavi_telemetry.phi_d_ap = ntohf( network_buffer.autopilot_field.phi_d_ap );
//        uavi_telemetry.theta_d_ap = ntohf( network_buffer.autopilot_field.theta_d_ap );
//        uavi_telemetry.psi_d_ap = ntohf( network_buffer.autopilot_field.psi_d_ap );
//        uavi_telemetry.p_d_ap = ntohf( network_buffer.autopilot_field.p_d_ap );
//        uavi_telemetry.q_d_ap = ntohf( network_buffer.autopilot_field.q_d_ap );
//        uavi_telemetry.r_d_ap = ntohf( network_buffer.autopilot_field.r_d_ap );
        break;
	case AFCS_STATE:	//afcs
		uavi_telemetry.gps_status = ntohs(network_buffer.afcs_field.gps_status);
		uavi_telemetry.radio_status = ntohs(network_buffer.afcs_field.radio_status);
		uavi_telemetry.ahrs_status = ntohs(network_buffer.afcs_field.ahrs_status);
		uavi_telemetry.takeoff_status = ntohs(network_buffer.afcs_field.takeoff_status);
		uavi_telemetry.autopilot_status = ntohs(network_buffer.afcs_field.autopilot_status);
		break;
	/*case FLYTO_MSG:   //flyto response -  this one doesn't seem to work?
		uavi_telemetry.transit_time = ntohs(network_buffer.flyto_field.transit_time);
		uavi_telemetry.flyto_pos_mode = ntohs(network_buffer.flyto_field.pos_mode);
		uavi_telemetry.flyto_hdg_mode = ntohs(network_buffer.flyto_field.hdg_mode);
		uavi_telemetry.flyto_n = ntohf(network_buffer.flyto_field.ft_n);
		uavi_telemetry.flyto_e = ntohf(network_buffer.flyto_field.ft_e);
		uavi_telemetry.flyto_d = ntohf(network_buffer.flyto_field.ft_d);
		uavi_telemetry.flyto_hdg = ntohf( network_buffer.flyto_field.ft_hdg);
		break;*/
	/*case TANGENT_PLANE:	//tangent plane llh -- this does not give llh, but delta l,l and alt
		uavi_telemetry.tangent_lat = ntohf(network_buffer.tangent_plane_field.tangent_lat);
		uavi_telemetry.tangent_lon = ntohf(network_buffer.tangent_plane_field.tangent_lon);
		uavi_telemetry.tangent_alt = ntohf(network_buffer.tangent_plane_field.tangent_alt);
		break;*/
	/*case DESIRED_POS:   //next desired point result  
		uavi_telemetry.n_d = ntohf(network_buffer.desired_pos_field.n_d);
		uavi_telemetry.e_d = ntohf(network_buffer.desired_pos_field.e_d);
		uavi_telemetry.d_d = ntohf(network_buffer.desired_pos_field.d_d);
		uavi_telemetry.hdg_d = ntohf(network_buffer.desired_pos_field.hdg_d);
		// if the command(s) are in the startup, just ignore them
		break;*/
	case GAIN_X:
		uavi_telemetry.velcap_xneg = ntohf(network_buffer.x_gain_field.x_min_outp);
		uavi_telemetry.velcap_xpos = ntohf(network_buffer.x_gain_field.x_max_outp);
		break;
	case GAIN_Y:
		uavi_telemetry.velcap_yneg = ntohf(network_buffer.y_gain_field.y_min_outp);
		uavi_telemetry.velcap_ypos = ntohf(network_buffer.y_gain_field.y_max_outp);
		break;
	case GAIN_Z:
		uavi_telemetry.velcap_zneg = ntohf(network_buffer.z_gain_field.z_min_outp);
		uavi_telemetry.velcap_zpos = ntohf(network_buffer.z_gain_field.z_max_outp);
		break;
		}
    }
}/////////////////////////////////////////////////////////////////////////////////////////////


//*****************************************************************************************
// send a data packet of data to the pilot computer;  user passes a buffer containing the packet
//******************************************************************************************
void send_to_pilot( char *buffer,  int buffersize)
{   int bytes_sent;
    
    if( verbose )	printf( "...start send to pilot....\n");

#ifdef WINDOWS
   bytes_sent = sendto( pilot_socket_fd, buffer, buffersize, 0,(struct sockaddr*)&pilot_address, sizeof( pilot_address ) );
#endif

#ifdef LINUX
   bytes_sent = sendto( pilot_socket_fd, (void*)buffer, buffersize, 0,(struct sockaddr*)&pilot_address, sizeof( pilot_address ) );
#endif
                        
  if( verbose )
  {	  printf( "%d bytes sent to IP %s at port %d...\n", bytes_sent, inet_ntoa( a ),
      ntohs( pilot_address.sin_port ) );
      printf( "...end send to pilot....\n" );      }
}//////////////////////////////////////////////////////////////////////////////////////////////

//***********************************************************************************
// use only for no-payload commands to the pilot - see rm_info.h for the codes
//************************************************************************************
void small_message_to_pilot(char code)     //12 bytes total, but the 11th is all we use
{    char connect_buffer[12];  // header is 12 bytes, last is command id

  if(!startup && verbose)
	  printf("Sending To Pilot\n");
  if(code==CLOSE_PILOT && !startup)
	  printf("Closing Pilot\n");
  memset(connect_buffer, 0, sizeof(connect_buffer));    // flush the buffer
  connect_buffer[11] = code;							// send open command to pilot
  send_to_pilot( connect_buffer, 12);					// send the connect command
}//////////////////////////////////////////////////////////////////////////////////////////////

//**********************************************************************************************8
// general flyto function - see rminfo.h for codes
// NOTE= you MUST choose the pos mode and hdg mode BEFORE calling this
// 3/17/05, working on R3 RM system, but velocity is ignored for now
//**************************************************************************************
void pilot_flyto( double north, double east, double down, double hdg)
{  union
	{ char raw[64];
		struct msg_flyto_t flyto_field;	
	} network_buffer;

  memset(network_buffer.raw, 0, 64 );
  //the "tv_sec" numbers are not really needed, but only to verify that each
  // command is unique, so a counter works just as well.  Dunno if they need to be different
  network_buffer.flyto_field.tv_sec = htonl( master_counter ) ;
  network_buffer.flyto_field.tv_usec = htonl( master_counter );
  master_counter++;  

  network_buffer.flyto_field.type = htonl(FLYTO_MSG);  // flyto ID
  network_buffer.flyto_field.pos_mode = htons( struct_vars.flyto_pos_mode );  // Parmeters in absolute meters
  network_buffer.flyto_field.hdg_mode = htons( struct_vars.flyto_hdg_mode );  // point nose in direction of travel

  //transit time in s or vel in cm/s if negative	 
  //network_buffer.flyto_field.transit_time = htons(struct_vars.desired_vel);  // send flyto command to pilot
  //network_buffer.flyto_field.transit_time = htons(-10000);  // send flyto command to pilot

  network_buffer.flyto_field.ft_n = htonf( north );
  network_buffer.flyto_field.ft_e = htonf( east );
  network_buffer.flyto_field.ft_d = htonf( down );
  network_buffer.flyto_field.ft_hdg = htonf( hdg  );

  send_to_pilot( network_buffer.raw, 40 + 12);   //40 bytes data, 12 bytes header
  
  if (verbose)
  {	  printf("Sending FLYTO N%8.4f  E%8.4f  D%8.4f  H%8.4f\n", north, east, down, hdg );
	  printf("main mode = %d   tail mode = %d   trans time = %d\n",struct_vars.flyto_pos_mode, struct_vars.flyto_hdg_mode, 1, 1);
  }
}///////////////////////////////////////////////////////////////////////////////////////////////

//***************************************************************************************8
// de-sensitzes gain/controls during FFF, which avoids wagging at speed
// needs testing
//***************************************************************************************88
void pilot_set_vel_scale(double vel_scale)
{	union 
	{	char raw[64];
		struct msg_velscale_t pilot_vel_scale;
	} network_buffer;

	if(!startup)
		printf("Sending vel scale update of %f\n", vel_scale);
	memset(network_buffer.raw, 0, sizeof(network_buffer));
	
	network_buffer.pilot_vel_scale.tv_sec = htonl(master_counter);
	network_buffer.pilot_vel_scale.tv_usec = htonl(master_counter);
	master_counter++;

	network_buffer.pilot_vel_scale.type = htonl(VEL_SCALE);  //packet ID
	network_buffer.pilot_vel_scale.vel_scale = htonf(vel_scale);
	send_to_pilot(network_buffer.raw, 40+12);  
}
////////////////////////////////////////////////////////////////////////////////////////////



